#include <Wire.h> 
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,16,2);

#include <Adafruit_NeoPixel.h>

#define LED_PIN     12
#define POWER_PIN   11
#define NUMPIXELS   1

Adafruit_NeoPixel pixels(NUMPIXELS, LED_PIN, NEO_GRB + NEO_KHZ800);

int id = 3;
String message = "";

void setup() {
  Serial.begin(9600);
  Serial1.begin(115200);
  pinMode(POWER_PIN, OUTPUT);
  digitalWrite(POWER_PIN, HIGH);
  pixels.begin();
  pixels.setBrightness(50);
  lcd.init();
  lcd.backlight();
}

void loop() {
  // Forward user input with FROM_ID tagging
  if (Serial.available() > 0) {
    String myInput = Serial.readStringUntil('\n');
    myInput.trim();

  
    myInput += (String)"#"+id+(String)";"; // Ensure the message ends correctly
    Serial1.println(myInput);

  }

  // Receive network message
  if (Serial1.available() > 0) {
    String incoming = Serial1.readStringUntil(';'); // Read until semicolon
    incoming.trim();

    Serial.println("Raw: " + incoming);

    // Extract command
    int colonIndex = incoming.indexOf(':');
    int atIndex = incoming.indexOf('@');
    int hashIndex = incoming.indexOf('#');

    if (colonIndex == -1 || atIndex == -1 || hashIndex == -1) return;

    String command = incoming.substring(0, colonIndex);
    String data = incoming.substring(colonIndex + 1, atIndex);
    int toID = incoming.substring(atIndex + 1, hashIndex).toInt();
    int fromID = incoming.substring(hashIndex + 1).toInt();

    Serial.println("Command: " + command);
    Serial.println("Data: " + data);
    Serial.println("To ID: " + String(toID));
    Serial.println("From ID: " + String(fromID));

    if (toID == id || toID == 0) {
      dispatchCommand(command, data);
    } else {
      //Serial.println("Relaying message...");
      //Serial1.println(incoming + ";"); // reappend delimiter

      Serial.println("Not my message...");
    }
  }
}

void dispatchCommand(String command, String data) {
  if (command == "COLOR") {
    int firstComma = data.indexOf(',');
    int secondComma = data.indexOf(',', firstComma + 1);

    if (firstComma == -1 || secondComma == -1) return;

    int r = data.substring(0, firstComma).toInt();
    int g = data.substring(firstComma + 1, secondComma).toInt();
    int b = data.substring(secondComma + 1).toInt();

    Serial.println("Setting color to R:" + String(r) + " G:" + String(g) + " B:" + String(b));
    setColor(r, g, b);

  }
  if (command == "DISPLAY"){
  lcd.clear();
  lcd.print(data);
}
  // Add more commands like BUZZ, MOVE, etc. here:
  else {
    Serial.println("Unknown command: " + command);
  }
}

void setColor(int r, int g, int b) {
  pixels.setPixelColor(0, pixels.Color(r, g, b));
  pixels.show();
}